function [DvSFB_m1_m, upsilon_l, upsilon_l1, DvScul_l, DvScul_l1] = bodycoordspecvelupdstep(mode, Dupsilon_lx, alpha_l1, Dalpha_lx, alpha_m, DvScul_m_ext) %#codegen
%BODYCOORDSPECVELUPDSTEP 体坐标系下比速度更新解算周期
%
% Input Arguments:
% mode: 标量，运行模式：0为初始化，1为高速解算周期，其它为中速解算周期
% Dupsilon_lx: 3*n矩阵，n为划桨效应补偿算法阶数，各列为高速解算周期内的比速度增量，第1列为本周期的，第2列为上一周期的，依此类推，单位m/s，1模式中调用
% alpha_l1, Dalpha_lx: 参见bodycoordattupdstep函数说明
% alpha_m: 3*1向量，本中速解算周期末的alpha_l，2模式中使用
% DvScul_m_ext：3*1向量，外部给出的划桨效应补偿项，如果不为空或者NaN，则在中速解算周期中使用该值替代内部计算的划桨效应补偿项，默认为空，2模式中使用
%
% Output Arguments:
% DvSFB_m1_m: 3*1向量，体坐标系下的比力积分增量，2模式中输出
% upsilon_l, upsilon_l1: 3*1向量，自本中速周期以来，本高速周期及上一高速周期的加速度计输出比速度增量的累积值，1模式中输出
% beta_l, beta_l1: 3*1向量，本高速周期及上一高速周期的划桨效应补偿项，1模式中输出
%
% References:
% [1] 理论文档“捷联惯性导航数值积分算法”， % 青鸟版本1.3 %

persistent p_mid_BCSVUS p_cfg_BCSVUS_fcndvScul_l p_cfg_BCSVUS_fcnDvRot_m

% persistent变量初始化
% NOTE: 不应依靠在该cell中对persistent变量赋的初值，因该cell仅在persistent变量不存在时执行
if isempty(p_mid_BCSVUS)
    p_mid_BCSVUS.upsilon_l = coder.nullcopy(NaN(3, 1));
    p_mid_BCSVUS.DvScul_l = coder.nullcopy(NaN(3, 1));
end
if isempty(p_cfg_BCSVUS_fcndvScul_l)
    p_cfg_BCSVUS_fcndvScul_l = @dvScul_l_2ndod;
end
if isempty(p_cfg_BCSVUS_fcnDvRot_m)
    p_cfg_BCSVUS_fcnDvRot_m = @DvRot_m_exactform_5thod;
end

if nargin < 6
    DvScul_m_ext = [];
end

switch mode
    case 0
        % 初始化
        p_mid_BCSVUS.upsilon_l = zeros(3, 1);
        p_mid_BCSVUS.DvScul_l = zeros(3, 1);
        DvSFB_m1_m = NaN(3, 1);
        upsilon_l1 = NaN(3, 1);
        DvScul_l1 = NaN(3, 1);
    case 1
        % 运行高速解算步骤
        upsilon_l1 = p_mid_BCSVUS.upsilon_l;
        DvScul_l1 = p_mid_BCSVUS.DvScul_l;
        [p_mid_BCSVUS.DvScul_l, p_mid_BCSVUS.upsilon_l] = vel_hs(DvScul_l1, upsilon_l1, Dupsilon_lx, alpha_l1, Dalpha_lx, p_cfg_BCSVUS_fcndvScul_l);
        DvSFB_m1_m = NaN(3, 1);
    otherwise
        % 运行中速解算步骤
        DvRot_m = p_cfg_BCSVUS_fcnDvRot_m(p_mid_BCSVUS.upsilon_l, alpha_m); % REF1式8.59
        if isempty(DvScul_m_ext) || any(isnan(DvScul_m_ext))            
            DvSFB_m1_m = p_mid_BCSVUS.upsilon_l + DvRot_m + p_mid_BCSVUS.DvScul_l; % REF1式8.56            
        else
            DvSFB_m1_m = p_mid_BCSVUS.upsilon_l + DvRot_m + DvScul_m_ext;
        end
        upsilon_l1 = NaN(3, 1);
        DvScul_l1 = NaN(3, 1);
end
upsilon_l = p_mid_BCSVUS.upsilon_l;
DvScul_l = p_mid_BCSVUS.DvScul_l;
end